# -*- coding: utf-8 -*-
"""
Created on Fri Sep  6 22:44:00 2024

@author: LENOVO
"""

import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd

#常数的确定
H=0.55
a=16*H  #m
b=H/(2*np.pi) #m
vh=1 #m/s
L0=(341-27.5*2)/100
Lb=(220-27.5*2)/100

r=lambda z:((a-b*(z)))




thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值

def theta(t):
    f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))-t
    theta=root(f,thetas(t))
    theta=theta.x[0]
    return theta

# 循环遍历
# for i in range(300,-1,-1):    #300s到0s循环遍历
# 第i秒位置函数

def position(i):
    Data=[]
    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))
    
    THETA.append(theta(i))
    X.append(r(THETA[0])*np.cos((THETA[0])))
    Y.append(-r(THETA[0])*np.sin((THETA[0])))

    Data.append(theta(i))
    f=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    Data.append(thetai)
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))

    A=np.column_stack((X, Y, THETA))

    return A

def Head(t):
   xh0=27.5/100
   yh0=15/100
   # xt0=-27.5/100
   # yt0=15/100
   P=position(t)
   k0=(P[0,1]-P[1,1])/(P[0,0]-P[1,0])
   a1=1/(np.sqrt(1+k0**2)) #cos
   a2=k0/(np.sqrt(1+k0**2)) #sin
   # A=np.array([[a1,-a2],[a2,a1]])
   xh=xh0*a1-yh0*a2+r(theta(t))*np.cos(theta(t))
   yh=xh0*a2+yh0*a1-r(theta(t))*np.sin(theta(t))
   H=np.array([xh,yh])
   return H

def distanceH(t):
    d=[]
    H=Head(t)
    P=position(t)
    for i in range(1,223):
       k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
       distance=abs(k*(H[0]-P[i,0])-H[1]+P[i,1])/np.sqrt(k**2+1)
       d.append(distance)
    return min(d)

def pitch(H):
    # gai=[]
    a=16*H  #m
    b=H/(2*np.pi)

    f=lambda theta:((a-b*(theta)))-4.5
    thet=root(f,43)
    thet=thet.x[0]
    x=thet
    t=(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))
    distance=distanceH(t)
    sub=distance-0.15
    return sub
f=lambda h:pitch(h)
h=root(f,0.15)
h=h.x[0]
print(h)
# x=np.arange(0.0,1.0,0.1)
# y=[]
# for i in np.arange(0.0,1.0,0.01):
#     a=pitch(i)
#     y.append(a)
# print(y)
# # plt.plot(x,y,lw=3,c='r',)
# # plt.grid()  
# # plt.show()

    
    